#ifndef MOVE_DRIVE_PLAN_H
#define MOVE_DRIVE_PLAN_H

#include<ros/ros.h>  
#include<geometry_msgs/Twist.h> 
#include <nav_msgs/Path.h> 

#include<iostream> 
#include<list>
#include<set> 
#include<math.h>
#include<stdio.h>



#include "std_msgs/UInt8MultiArray.h"
#include "std_msgs/UInt8.h"

#include "TrackPath.h"
using namespace std;  

namespace ljl
{
class TRACKPATH;
class BOOSTPATH;

class MOVE_PLAN_SYS  {  
TRACKPATH* mTrackPath;
BOOSTPATH* mBoostPath;	 //out: mBoostPath->mvTrackPaths
public:  
    MOVE_PLAN_SYS(TRACKPATH* TrackPath, 
		BOOSTPATH* BoostPath, 
		string filename); 
    MOVE_PLAN_SYS(){};
    ~MOVE_PLAN_SYS(){};  
  
    /* subscrib pose of camera
       *assign robot_x, robot_y, robot_w from msg		   ^ /robot_y
       *robot_x, robot_y: two dimensional coordinates of robot in  |
								   | 
								   |_________> /robot_x
	*robot_x = x of camera
	*robot_y = z of camera

    */
    void poseCallback(const geometry_msgs::PoseStampedConstPtr& msg);

    /*
	load trackpath.bin from location
    */
    void loadGoalPose();
    float wad2ang(const float w);

    /*
	*input:	x, y, theta
	*output:	v: linear speed for forward
	*output:	ang: angular speed for turning

    */
	bool xycovert2vw(const float x_start,
					const float y_start,
					const float theta_start,
					const float x_goal,
					const float y_goal,
					const float theta_goal,
					float &v,
					float &w
					);

    //fast stop
    void fast_stop(ros::Publisher &pub_cmd, geometry_msgs::Twist &cmd);
    //slow stop
    void slow_stop(ros::Publisher &pub_cmd, geometry_msgs::Twist &cmd);

    void slow_start(geometry_msgs::Twist &cmd, float &control_speed, float &control_turn);

public:
     std::vector<std::vector<float> > mvTrackPaths;

    double mrobot_x;
    double mrobot_y;
    double mrobot_w;

    bool miscallback = false;

    float mstatus_v = 0.0;
    float mstatus_w = 0.0;
private:  



//    double goal_x;
//    double goal_y;
//    double goal_w;

    string mposePath;
    geometry_msgs::PoseStamped mpose;


	//simulation parameters
	float mKp_rho = 9;
	float mKp_alpha = 15;
	float mKp_beta = -3;
	float mdt = 0.01;
	
	double mPI = 3.1415926;
	float m180 = 180.0;

	vector<float> mdxy;
	bool misback = false;

	float mMaxv = 1.0;	//2
	
	float mMinw = 0.1;
	float mMaxw = 1.0;	//2

	vector<float> mvspeed;

	

    

};
}
#endif
